/*
 *  updateTrajectory.cpp
 *  Ball and Plate
 *
 *  Created by Kenn Sebesta on 2007-11-28.
 *  Copyright 2007 __MyCompanyName__. All rights reserved.
 *
 */

#include "math.h"
#include "chaseTrajectories.h"
#include "proximityTrajectories.h"
#include "displayOpenCV.h"
#include "updateSetPoint.h"
//#include "updateTrajectory.h"

enum paths {override, //ALWAYS KEEP override first
stabilize, circleCh, ellipseCh, spiralCh, figure_eightCh, customCh,
circleProx, customProx,
enum_sentinel}; //ALWAYS KEEP enum_sentinel last

const int number_of_paths = enum_sentinel-1;
extern CvRect limitR;

void stabilization( SetPoint *, MaskObject, double);

void updateSetPoint( SetPoint *setpoint, MaskObject *shapes, MaskObject ball, CvMat *customTrajectory, int numMarkers, double runTime )
// Should have some normalization wrt the sampling rate.
{
    
    if ( setpoint->pathNum == override ) {
        if ( strcmp(setpoint->pathName, "findMarkers")==0 ){
            float magnitude=ball.diaPix*3.14/2;
//            float direction[]={0, 0};
            float dx=0, dy=0;
            for (int i=1; i< numMarkers; i++){
                if( shapes[i].found ){
                    dx+=ball.pixCoords[0]-shapes[i].pixCoords[0];
                    dy+=ball.pixCoords[1]-shapes[i].pixCoords[1];
                }
            }
            float norm= sqrt(dx*dx+dy*dy);
            float tmpX=ball.pixCoords[0]+dx*magnitude/norm;
            float tmpY=ball.pixCoords[1]+dy*magnitude/norm;
            if (tmpX < limitR.x) //If the ball will leave the side, make it go the other way
                cvmSet(setpoint->setPoint, 0, 0, ball.pixCoords[0]-dx*magnitude/norm);
            else if (tmpX > (limitR.x+limitR.width)) //If the ball will leave the side, make it go the other way
                cvmSet(setpoint->setPoint, 0, 0, ball.pixCoords[0]-dx*magnitude/norm);
            else
                cvmSet(setpoint->setPoint, 0, 0, tmpX);
            
            if (tmpY < limitR.y) //If the ball will leave the top, make it go the other way
                cvmSet(setpoint->setPoint, 1, 0, ball.pixCoords[1]-dy*magnitude/norm);
            else if (tmpY > (limitR.y+limitR.height))  //If the ball will leave the bottom, make it go the other way
                cvmSet(setpoint->setPoint, 1, 0, ball.pixCoords[1]-dy*magnitude/norm);
            else
                cvmSet(setpoint->setPoint, 1, 0, tmpY);
        }
        else{
            
            strcpy(setpoint->pathName, "stabilization");
            stabilization( setpoint, ball, runTime );
            
        }
    }
    
    else{
        
        switch( setpoint->pathNum % number_of_paths +1 ){
            case circleCh:
                
                strcpy(setpoint->pathName, "circleChase");
                circleChase( setpoint, runTime );
                break;
            case ellipseCh:
                strcpy(setpoint->pathName, "ellipseChase");
                ellipseChase( setpoint, runTime );
                break;
            case spiralCh:
                strcpy(setpoint->pathName, "spiralChase");
                spiralChase( setpoint, runTime );
                break;
            case figure_eightCh:
                strcpy(setpoint->pathName, "figure-eightChase");
                figure_eightChase( setpoint, runTime );
                break;
            case customCh:
                strcpy(setpoint->pathName, "custom");
//            customChase( runTime, ballSpeed, pathRadius, integrationErr, setPoint, tableCenter );
                break;
            case circleProx:
                strcpy(setpoint->pathName, "circleProximity");
                circleProximity( setpoint, ball, runTime );
                break;
            case customProx:
                strcpy(setpoint->pathName, "customProximity");
            customProximity( setpoint, ball, customTrajectory);
                break;
            default:
                strcpy(setpoint->pathName, "stabilization");
                stabilization( setpoint, ball, runTime );
        }
    }
    
    return;
}

void stabilization( SetPoint *setpoint, MaskObject ball, double runTime ) {
    cvZero( setpoint->setPoint );
    cvmSet( setpoint->setPoint, 0, 0, cvmGet( setpoint->tableCenter, 0, 0 ) );
    cvmSet( setpoint->setPoint, 2, 0, cvmGet( setpoint->tableCenter, 0, 1 ) );
    
    //Add integrator
    CvMat *errPos=cvCreateMat( 2, 1, CV_32F );
    CvMat *errVel=cvCreateMat( 2, 1, CV_32F );

    cvmSet( errPos, 0, 0, cvmGet(ball.state, 0, 0) - cvmGet(setpoint->setPointR, 0, 0) );
    cvmSet( errPos, 1, 0, cvmGet(ball.state, 2, 0) - cvmGet(setpoint->setPointR, 2, 0) );
    cvmSet( errVel, 0, 0, cvmGet(ball.state, 1, 0) - cvmGet(setpoint->setPointR, 1, 0) );
    cvmSet( errVel, 1, 0, cvmGet(ball.state, 3, 0) - cvmGet(setpoint->setPointR, 3, 0) );
    
    cvAdd(setpoint->integrationPositionErr, errPos, setpoint->integrationPositionErr);
    cvAdd(setpoint->integrationVelocityErr, errVel, setpoint->integrationVelocityErr);
       
    //Garbage collection
    cvReleaseMat( &errPos );
    cvReleaseMat( &errVel );
    
    return;
}
